﻿using OnDash.Mathematics.Data;
using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Matrix;

public struct Matrix4X3 : IEquatable<Matrix4X3>
{
    private Vector3 _row0;

    public Vector3 Row0
    {
        get => _row0;
        set => _row0 = value;
    }

    private Vector3 _row1;

    public Vector3 Row1
    {
        get => _row1;
        set => _row1 = value;
    }

    private Vector3 _row2;

    public Vector3 Row2
    {
        get => _row2;
        set => _row2 = value;
    }

    private Vector3 _row3;

    public Vector3 Row3
    {
        get => _row3;
        set => _row3 = value;
    }

    public Matrix4X3(Vector3 row0, Vector3 row1, Vector3 row2, Vector3 row3)
    {
        _row0 = row0;
        _row1 = row1;
        _row2 = row2;
        _row3 = row3;
    }

    public Matrix4X3(float m00, float m01, float m02, float m03,
        float m10, float m11, float m12, float m13,
        float m20, float m21, float m22, float m23)
    {
        _row0 = new Vector3(m00, m01, m02);
        _row1 = new Vector3(m10, m11, m12);
        _row2 = new Vector3(m20, m21, m22);
        _row3 = new Vector3(m03, m13, m23);
    }

    public Vector4 Column0
    {
        get => new(M11, M21, M31, M41);
        set
        {
            M11 = value.X;
            M21 = value.Y;
            M31 = value.Z;
            M41 = value.W;
        }
    }

    public Vector4 Column1
    {
        get => new(M12, M22, M32, M42);
        set
        {
            M12 = value.X;
            M22 = value.Y;
            M32 = value.Z;
            M42 = value.W;
        }
    }

    public Vector4 Column2
    {
        get => new(M13, M23, M33, M43);
        set
        {
            M13 = value.X;
            M23 = value.Y;
            M33 = value.Z;
            M43 = value.W;
        }
    }

    public float M11
    {
        get => _row0.X;
        set => _row0.X = value;
    }

    public float M12
    {
        get => _row0.Y;
        set => _row0.Y = value;
    }

    public float M13
    {
        get => _row0.Z;
        set => _row0.Z = value;
    }

    public float M21
    {
        get => _row1.X;
        set => _row1.X = value;
    }

    public float M22
    {
        get => _row1.Y;
        set => _row1.Y = value;
    }

    public float M23
    {
        get => _row1.Z;
        set => _row1.Z = value;
    }

    public float M31
    {
        get => _row2.X;
        set => _row2.X = value;
    }

    public float M32
    {
        get => _row2.Y;
        set => _row2.Y = value;
    }

    public float M33
    {
        get => _row2.Z;
        set => _row2.Z = value;
    }

    public float M41
    {
        get => _row3.X;
        set => _row3.X = value;
    }

    public float M42
    {
        get => _row3.Y;
        set => _row3.Y = value;
    }

    public float M43
    {
        get => _row3.Z;
        set => _row3.Z = value;
    }

    public Vector3 Diagonal
    {
        get => new(M11, M22, M33);
        set
        {
            M11 = value.X;
            M22 = value.Y;
            M33 = value.Z;
        }
    }

    public float Trace => M11 + M22 + M33;

    public float this[int rowIndex, int columnIndex]
    {
        get
        {
            return rowIndex switch
            {
                0 => _row1[columnIndex],
                1 => _row1[columnIndex],
                2 => _row1[columnIndex],
                3 => _row2[columnIndex],
                _ => throw new IndexOutOfRangeException(
                    $"You tried to access this matrix at: ({rowIndex}, {columnIndex})")
            };
        }
        set
        {
            switch (rowIndex)
            {
                case 0:
                    _row0[columnIndex] = value;
                    break;
                case 1:
                    _row1[columnIndex] = value;
                    break;
                case 2:
                    _row2[columnIndex] = value;
                    break;
                case 3:
                    _row3[columnIndex] = value;
                    break;
                default:
                    throw new IndexOutOfRangeException(
                        $"You tried to access this matrix at: ({rowIndex}, {columnIndex})");
            }
        }
    }

    public Matrix4X3 Invert() => Invert(this);

    public static Matrix4X3 Invert(Matrix4X3 matrix)
    {
        var inverseRotation = new Matrix3(matrix.Column0.Xyz, matrix.Column1.Xyz, matrix.Column2.Xyz);
        inverseRotation.Row0 /= inverseRotation.Row0.LengthSquared;
        inverseRotation.Row1 /= inverseRotation.Row1.LengthSquared;
        inverseRotation.Row2 /= inverseRotation.Row2.LengthSquared;

        var translation = matrix.Row3;

        return new Matrix4X3
        (
            inverseRotation.M11, inverseRotation.M12, inverseRotation.M13,
            inverseRotation.M21, inverseRotation.M22, inverseRotation.M23,
            inverseRotation.M31, inverseRotation.M32, inverseRotation.M33,
            -Vector3.Dot(inverseRotation.Row0, translation),
            -Vector3.Dot(inverseRotation.Row1, translation),
            -Vector3.Dot(inverseRotation.Row2, translation)
        );
    }

    public static Matrix3X4 Transpose(Matrix4X3 matrix)
    {
        return new Matrix3X4
        (
            matrix.M11, matrix.M21, matrix.M31, matrix.M41,
            matrix.M12, matrix.M22, matrix.M32, matrix.M42,
            matrix.M13, matrix.M23, matrix.M33, matrix.M43
        );
    }

    public static Matrix4X3 Add(Matrix4X3 left, Matrix4X3 right)
    {
        return new Matrix4X3
        (
            left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13,
            left.M21 + right.M21, left.M22 + right.M22, left.M23 + right.M23,
            left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33,
            left.M41 + right.M41, left.M42 + right.M42, left.M43 + right.M43
        );
    }

    public static Matrix4X3 Subtract(Matrix4X3 left, Matrix4X3 right)
    {
        return new Matrix4X3
        (
            left.M11 - right.M11, left.M12 - right.M12, left.M13 - right.M13,
            left.M21 - right.M21, left.M22 - right.M22, left.M23 - right.M23,
            left.M31 - right.M31, left.M32 - right.M32, left.M33 - right.M33,
            left.M41 - right.M41, left.M42 - right.M42, left.M43 - right.M43
        );
    }

    public static Matrix4X3 Multiply(Matrix4X3 matrix, float scale)
    {
        return new Matrix4X3
        (
            matrix.M11 * scale, matrix.M12 * scale, matrix.M13 * scale,
            matrix.M21 * scale, matrix.M22 * scale, matrix.M23 * scale,
            matrix.M31 * scale, matrix.M32 * scale, matrix.M33 * scale,
            matrix.M41 * scale, matrix.M42 * scale, matrix.M43 * scale
        );
    }

    public static Matrix4 Multiply(Matrix4X3 left, Matrix3X4 right)
    {
        return new Matrix4
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
            left.M11 * right.M14 + left.M12 * right.M24 + left.M13 * right.M34,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
            left.M21 * right.M14 + left.M22 * right.M24 + left.M23 * right.M34,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33,
            left.M31 * right.M14 + left.M32 * right.M24 + left.M33 * right.M34,
            left.M41 * right.M11 + left.M42 * right.M21 + left.M43 * right.M31,
            left.M41 * right.M12 + left.M42 * right.M22 + left.M43 * right.M32,
            left.M41 * right.M13 + left.M42 * right.M23 + left.M43 * right.M33,
            left.M41 * right.M14 + left.M42 * right.M24 + left.M43 * right.M34
        );
    }

    public static Matrix4X3 Multiply(Matrix4X3 left, Matrix4X3 right)
    {
        return new Matrix4X3
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31 + right.M41,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32 + right.M42,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33 + right.M43,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31 + right.M41,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32 + right.M42,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33 + right.M43,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31 + right.M41,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32 + right.M42,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33 + right.M43,
            left.M41 * right.M11 + left.M42 * right.M21 + left.M43 * right.M31 + right.M41,
            left.M41 * right.M12 + left.M42 * right.M22 + left.M43 * right.M32 + right.M42,
            left.M41 * right.M13 + left.M42 * right.M23 + left.M43 * right.M33 + right.M43
        );
    }

    public static Matrix4X3 CreateFromAxisAngle(Vector3 axis, float angle)
    {
        axis.Normalize();

        var cos = MathHelper.Cos(-angle);
        var sin = MathHelper.Sin(-angle);

        return new Matrix4X3
        (
            1.0f - cos * axis.X * axis.X + cos,
            1.0f - cos * axis.X * axis.Y - sin * axis.Z,
            1.0f - cos * axis.X * axis.Z + sin * axis.Y,
            1.0f - cos * axis.X * axis.Y + sin * axis.Z,
            1.0f - cos * axis.Y * axis.Y + cos,
            1.0f - cos * axis.Y * axis.Z - sin * axis.X,
            1.0f - cos * axis.X * axis.Z - sin * axis.Y,
            1.0f - cos * axis.Y * axis.Z + sin * axis.X,
            1.0f - cos * axis.Z * axis.Z + cos,
            0,
            0,
            0
        );
    }

    public static Matrix4X3 CreateFromQuaternion(Quaternion quaternion)
    {
        return new Matrix4X3
        (
            1f - 2 * quaternion.Y * quaternion.Y - 2 * quaternion.Z * quaternion.Z,
            2 * quaternion.X * quaternion.Y - 2 * quaternion.W * quaternion.Z,
            2 * quaternion.X * quaternion.Z + 2 * quaternion.W * quaternion.Y,
            2 * quaternion.X * quaternion.Y + 2 * quaternion.W * quaternion.Z,
            1f - 2 * quaternion.X * quaternion.X - 2 * quaternion.Z * quaternion.Z,
            2 * quaternion.Y * quaternion.Z - 2 * quaternion.W * quaternion.X,
            2 * quaternion.X * quaternion.Z - 2 * quaternion.W * quaternion.Y,
            2 * quaternion.Y * quaternion.Z + 2 * quaternion.W * quaternion.X,
            1f - 2 * quaternion.X * quaternion.X - 2 * quaternion.Y * quaternion.Y,
            0, 0, 0
        );
    }

    public static Matrix4X3 CreateRotationX(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4X3
        (
            1, 0, 0,
            0, cos, sin,
            0, -sin, cos,
            0, 0, 0
        );
    }

    public static Matrix4X3 CreateRotationY(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4X3
        (
            cos, 0, -sin,
            0, 1, 0,
            sin, 0, cos,
            0, 0, 0
        );
    }

    public static Matrix4X3 CreateRotationZ(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4X3
        (
            cos, sin, 0,
            -sin, cos, 0,
            0, 0, 1,
            0, 0, 0
        );
    }

    public static Matrix4X3 CreateTranslation(float x, float y, float z)
    {
        return new Matrix4X3
        (
            1, 0, 0,
            0, 1, 0,
            0, 0, 1,
            x, y, z
        );
    }

    public static Matrix4X3 CreateTranslation(Vector3 vector)
        => CreateTranslation(vector.X, vector.Y, vector.Z);

    public static Matrix4X3 CreateScale(float x, float y, float z)
    {
        return new Matrix4X3
        (
            x, 0, 0,
            0, y, 0,
            0, 0, z,
            0, 0, 0
        );
    }

    public static Matrix4X3 CreateScale(Vector3 vector) => CreateScale(vector.X, vector.Y, vector.Z);
    public static Matrix4X3 CreateScale(float scale) => CreateScale(scale, scale, scale);

    public static Matrix4X3 operator +(Matrix4X3 left, Matrix4X3 right) => Add(left, right);
    public static Matrix4X3 operator -(Matrix4X3 left, Matrix4X3 right) => Subtract(left, right);
    public static Matrix4 operator *(Matrix4X3 left, Matrix3X4 right) => Multiply(left, right);
    public static Matrix4X3 operator *(Matrix4X3 left, Matrix4X3 right) => Multiply(left, right);
    public static Matrix4X3 operator *(Matrix4X3 left, float right) => Multiply(left, right);
    public static Matrix4X3 operator *(float left, Matrix4X3 right) => Multiply(right, left);

    public static bool operator ==(Matrix4X3 left, Matrix4X3 right) => left.Equals(right);
    public static bool operator !=(Matrix4X3 left, Matrix4X3 right) => !left.Equals(right);

    public override string ToString() => $"{_row0}\n{_row1}\n{_row2}\n{_row3}";

    public override int GetHashCode() => HashCode.Combine(_row0, _row1, _row2, _row3);

    public override bool Equals(object? obj) => obj is Matrix4X3 other && Equals(other);

    public bool Equals(Matrix4X3 other)
    {
        return _row0.Equals(other._row0) &&
               _row1.Equals(other._row1) &&
               _row2.Equals(other._row2) &&
               _row3.Equals(other._row3);
    }
}